#pragma once

#include "CAction.h"

#define TORQUECONTROL_ID 17
using namespace std;



class CTorqueControl :
	public CAction
{
public:
	CTorqueControl(void);
	CTorqueControl( CSeaAmp &seaAmp ) { Init( seaAmp ); }
	~CTorqueControl(void);

	const Error *EnableAction(void);

	bool Init( CSeaAmp &seaAmp ) { return CAction::Init( seaAmp, TORQUECONTROL_ID ); }

	void DoAct();

	void SetKP( double p ) { Kp = p; }
	double GetKP( void ) { return Kp; }

	void SetKI( double i ) { Ki = i; }
	double GetKI( void ) { return Ki; }

	void SetKD( double d ) { Kd = d; }
	double GetKD( void ) { return Kd; }

	void SetAdjustParaA( double a ) { adjustParaA = a; }
	double GetAdjustParaA( void ) { return adjustParaA; }
	
	void SetAdjustParaB( double b ) { adjustParaB = b; }
	double GetAdjustParaB( void ) { return adjustParaB; }

	void SetAdjustSwitch( bool s ) { adjustSwitch = s; }
	bool GetAdjustSwitch( void ) { return adjustSwitch; }

	void SetOutputLimit( double ol) { outputLimit = ol;}

private:

	double Kp;
	double Ki;
	double Kd;

	double lastSet;

	double delta[2];

	double velMotorActual;

	double output;
	double lastOutput;

	double adjustOutput;
	double lastAdjustOutput;

	double adjustParaA;
	double adjustParaB;

	double outputLimit;

	bool adjustSwitch;

	ofstream f_control;
	ofstream f_measure;

};


